package pteam;

import simulator.Actor;
import simulator.Robot;
import simulator.Sensor;
import simulator.Simulator;

public class GoldNavActor implements Actor{
	public static final int PID_CONSTANT = 5;
	public static final int DRIVE_DIST = 5;
	public static final int OBJECT_DRIVE_DIST = 20;
	public static final double MIN_CLEARANCE = 30;
	public static final int RF_MOUNT_ANGLE = 30;
	public static final int TURN_AROUND_ANGLE = 45;
	public static final int FIX_DIVIDER = 10;
	public int deadEndCount = 0;
	public boolean turnAroundLeft;
	public double frontRange;
	public double beaconHeading = 0;
	public double leftRange;
	public double rightRange;
	public double heading;
	public boolean initializing = true;
	public boolean haveData = false;
	
	
	
	//take out heading fixes. /////////////////////////////
	
	public void act(Robot s) {
		
		if(initializing) {
			
			if(haveData){
				beaconHeading += heading;
				initializing = false;
				haveData = false;
			}
		} else if(haveData){
			// if object in front
			if(frontRange < MIN_CLEARANCE){
				// somehow avoid the object
				avoidObject(s);
			} else {
				deadEndCount = 0;
				// no object
				System.out.println("no object");
				s.goForward();
				s.waitDistance(DRIVE_DIST);
				int pidAngle = (int) 
					(findOptimal(heading, beaconHeading)/PID_CONSTANT);
				System.out.println("PID: " + pidAngle + " h: " + heading +  " bh: " + beaconHeading);
				if(pidAngle > 0)
				{
					s.turnLeft();
					s.waitAngle(pidAngle);
					
				}else{
					s.turnRight();
					s.waitAngle(-1 * pidAngle);
				}
			}
			haveData = false;
		}
		
	}
	
	public void avoidObject(Robot s)
	{
		// check...
		// 1. if either range is longer than minimum range. if not turn around
		// 2. go towards the bigger of the two
		if(leftRange < MIN_CLEARANCE + 10 && rightRange < MIN_CLEARANCE + 10){
			if(deadEndCount == 0)
			{
				//turnAroundLeft = findOptimal(heading, beaconHeading) > 0;
				if(leftRange > rightRange)
				{
					turnAroundLeft = true;
				} else {
					turnAroundLeft = false;
				}
			}
			deadEndCount++;
			if(turnAroundLeft){
				s.turnLeft();
			}else{
				s.turnRight();
			}
			turnAroundLeft = !(turnAroundLeft);
			s.waitAngle(TURN_AROUND_ANGLE * deadEndCount);
		} else {
			deadEndCount = 0;
			double fix = findOptimal(heading, beaconHeading) / FIX_DIVIDER;
			double rightScore = rightRange;
			double leftScore = leftRange;
			if(leftRange > MIN_CLEARANCE + 10 && rightRange > MIN_CLEARANCE + 10)
			{
				if(fix > 0) {
					leftScore += fix;
				} else {
					rightScore -= fix;
				}
			}
			
			if(rightScore > leftScore) {
				s.turnRight();
				s.waitAngle(RF_MOUNT_ANGLE);
			} else {
				s.turnLeft();
				s.waitAngle(RF_MOUNT_ANGLE);
			}
			s.goForward();
			s.waitDistance(OBJECT_DRIVE_DIST);
		}
	}
	
	//finds the optimal way to turn from the currentHeading to targetHeading
	public double findOptimal(double currentHeading, double targetHeading)
	{
		if(targetHeading == currentHeading)
		{
			return 0;
		}
		if(targetHeading > currentHeading)
		{
			if(Math.abs(currentHeading - targetHeading) < 
					Math.abs(currentHeading + 360 - targetHeading))
			{
				return targetHeading - currentHeading;
			} else {
				return -1 * (currentHeading + 360 - targetHeading);
			}
			
		}else{
			if(Math.abs(currentHeading - targetHeading) < 
					Math.abs(currentHeading - 360 - targetHeading))
			{
				return targetHeading - currentHeading;
			} else {
				return -1 * (currentHeading - 360 - targetHeading);
			}
		}
	}

	public String getMaze() {
		return "GoldGen";
	}

	public void updateSensors(Sensor[] sensors) {
		// TODO Auto-generated method stub
		leftRange = sensors[2].getState()[0];
		rightRange = sensors[4].getState()[0];
		frontRange = sensors[1].getState()[0];
		haveData = true;
		double compassHeading = sensors[7].getState()[0]%360;
		if(compassHeading >= 360){
			compassHeading -= 360;
		}
		if(compassHeading < 0){
			compassHeading += 360;
		}
		compassHeading = 360 - compassHeading;
		//System.out.println(compassHeading);
		heading = compassHeading;
	}

}
